// ========================= tcpserver.c =========================
#ifdef _WIN32
#define _WINSOCK_DEPRECATED_NO_WARNINGS
#include <winsock2.h>
#include <ws2tcpip.h>
#pragma comment(lib, "ws2_32.lib")
#include <windows.h>
#else
#include <unistd.h>
#include <fcntl.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <errno.h>
#include <pthread.h>
#endif
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>

#include "tcpserver.h"
#include "ethercatWindows.h"



static volatile int g_running = 0;
static int g_srv = -1;
static int g_cli = -1; // single client only

#ifdef _WIN32
#include <string.h>
#include <ctype.h>

// Windows 下 strcasecmp 替代
int strcasecmp(const char *s1, const char *s2) {
    return _stricmp(s1, s2);
}

// Windows 下 strncasecmp 替代
int strncasecmp(const char *s1, const char *s2, size_t n) {
    return _strnicmp(s1, s2, n);
}

// Windows 下 strcasestr 替代
char *strcasestr(const char *haystack, const char *needle) {
    if (!*needle) return (char *)haystack;
    size_t needle_len = strlen(needle);
    for (; *haystack; haystack++) {
        if (_strnicmp(haystack, needle, needle_len) == 0) {
            return (char *)haystack;
        }
    }
    return NULL;
}
#endif

// Threads
#ifdef _WIN32
static HANDLE th_accept = NULL;
static HANDLE th_recv = NULL;
static HANDLE th_send = NULL;
#else
static pthread_t th_accept, th_recv, th_send;
#endif

// ---- small helpers ----
static void sleep_ms(int ms)
{
#ifdef _WIN32
    Sleep(ms);
#else
    struct timespec ts = {ms / 1000, (ms % 1000) * 1000000L};
    nanosleep(&ts, NULL);
#endif
}

static void closesock_safe(int *fd)
{
    if (*fd >= 0)
    {
#ifdef _WIN32
        closesocket(*fd);
#else
        close(*fd);
#endif
        *fd = -1;
    }
}

static int socket_set_nonblock(int fd)
{
#ifdef _WIN32
    u_long mode = 1;
    return ioctlsocket(fd, FIONBIO, &mode);
#else
    int flags = fcntl(fd, F_GETFL, 0);
    if (flags < 0) return -1;
    return fcntl(fd, F_SETFL, flags | O_NONBLOCK);
#endif
}

// ---- command parser ----
static void handle_command(const char *buf)
{
    char cmd[64] = {0};
    int slaveIndex = -1,passiveIndex = -1, id = -1, value = -1;
    int target = 0, vel = 0, acc = 0, dec = 0;

    //printf("[CMD] raw: %s\n", buf);

    // 提取 cmd
    const char *p = strcasestr(buf, "cmd");
    if (p) {
        sscanf(p, "%*[^:]:%63[^,}]", cmd);
        // 去掉前后的引号
        size_t len = strlen(cmd);
        if (len > 0 && cmd[0] == '"') {
            memmove(cmd, cmd + 1, len);  // 左移去掉首引号
            len--;
        }
        if (len > 0 && cmd[len - 1] == '"') {
            cmd[len - 1] = '\0';         // 去掉尾引号
        }
    }

    // 提取常见参数
    if ((p = strcasestr(buf, "slaveindex")) || (p = strcasestr(buf, "slaveIndex")))
        sscanf(p, "%*[^:]:%d", &slaveIndex);
    if ((p = strcasestr(buf, "passiveindex")) || (p = strcasestr(buf, "passiveIndex")))
        sscanf(p, "%*[^:]:%d", &passiveIndex);
    if ((p = strcasestr(buf, "id")))
        sscanf(p, "%*[^:]:%d", &id);
    if ((p = strcasestr(buf, "value")))
        sscanf(p, "%*[^:]:%d", &value);
    if ((p = strcasestr(buf, "target")))
        sscanf(p, "%*[^:]:%d", &target);
    if ((p = strcasestr(buf, "velocity")))
        sscanf(p, "%*[^:]:%d", &vel);
    if ((p = strcasestr(buf, "acc")))
        sscanf(p, "%*[^:]:%d", &acc);
    if ((p = strcasestr(buf, "dec")) || (p = strcasestr(buf, "decc")))
        sscanf(p, "%*[^:]:%d", &dec);

    //printf("[CMD] parsed: cmd=%s, slaveIndex=%d, id=%d, value=%d, "
    //       "target=%d, vel=%d, acc=%d, dec=%d\n",
    //       cmd, slaveIndex, id, value, target, vel, acc, dec);

    // 分发命令
    if (strcasecmp(cmd, "SetDO") == 0)
    {
        printf("[CMD] -> SetDO\n");
        if (slaveIndex >= 0 && id >= 0 && value >= 0)
        {
            //printf("  slave=%d, id=%d, value=%d\n", slaveIndex, id, value);
            ethercatw_set_do(slaveIndex, id, value);
        }
        else
        {
            printf("  [WARN] invalid SetDO parameters!\n");
        }
    }
    else if (strcasecmp(cmd, "MoveAxis") == 0)
    {
        printf("[CMD] -> MoveAxis\n");
        if (slaveIndex >= 0)
        {
            printf("  slave=%d, target=%d, vel=%d, acc=%d, dec=%d\n",
                   slaveIndex, target, vel, acc, dec);
            
            ethercatw_move_axis(slaveIndex, target, vel, acc, dec);
        }
        else
        {
            printf("  [WARN] invalid MoveAxis parameters!\n");
        }
    }
    else if (strcasecmp(cmd, "Move2Axes") == 0)
    {
        printf("[CMD] -> Move2Axes\n");
        if (slaveIndex >= 0)
        {
            printf("  slave=%d, slave2=%d, target=%d, vel=%d, acc=%d, dec=%d\n",
                   slaveIndex, passiveIndex, target, vel, acc, dec);
            
            ethercatw_move_2axes(slaveIndex,passiveIndex, target, vel, acc, dec);
        }
        else
        {
            printf("  [WARN] invalid Move2Axes parameters!\n");
        }
    }
    else if (strcasecmp(cmd, "StopAxis") == 0)
    {
        printf("[CMD] -> StopAxis\n");
        if (slaveIndex >= 0)
        {
            printf("  slave=%d\n", slaveIndex);
            ethercatw_stop_axis(slaveIndex);
        }
        else
        {
            printf("  [WARN] invalid StopAxis parameters!\n");
        }
    }
    else
    {
        printf("[CMD] Unknown command: %s\n", cmd);
    }
}


// ---- thread functions ----
#ifdef _WIN32
static DWORD WINAPI th_accept_fn(LPVOID arg)
#else
static void *th_accept_fn(void *arg)
#endif
{
    struct sockaddr_in cliaddr;
    socklen_t clilen = sizeof(cliaddr);

    while (g_running)
    {
        int c = accept(g_srv, (struct sockaddr *)&cliaddr, &clilen);
        if (c >= 0)
        {
            if (g_cli >= 0)
            {
                closesock_safe(&c); // already have client
            }
            else
            {
                g_cli = c;
                printf("Client connected.\n");
            }
        }
        sleep_ms(100);
    }
    return 0;
}

#ifdef _WIN32
static DWORD WINAPI th_recv_fn(LPVOID arg)
#else
static void *th_recv_fn(void *arg)
#endif
{
    char buf[1024];
    while (g_running)
    {
        if (g_cli >= 0)
        {
            int n = recv(g_cli, buf, sizeof(buf) - 1, 0);
            if (n > 0)
            {
                buf[n] = 0;
                handle_command(buf);
            }
            else if (n == 0)
            {
                printf("Client disconnected.\n");
                closesock_safe(&g_cli);
            }
        }
        sleep_ms(10);
    }
    return 0;
}

#ifdef _WIN32
static DWORD WINAPI th_send_fn(LPVOID arg)
#else
static void *th_send_fn(void *arg)
#endif
{
    Device snap[MAX_DEVICES];
    char out[2048];

    while (g_running)
    {
        int n = ethercatw_get_snapshot(snap, MAX_DEVICES);
        if (g_cli >= 0)
        {
            // 简单输出 JSON 行
            strcpy(out, "{devices:[");
            for (int i = 0; i < n; i++)
            {
                char tmp[256];
                if (snap[i].type == DEV_IO)
                {
                    snprintf(tmp, sizeof(tmp),
                             //"{name:\"%s\", index:%d, DO:[%d,%d,%d,%d,%d,%d,%d,%d, %d,%d,%d,%d,%d,%d,%d,%d], DI:[%d,%d,%d,%d,%d,%d,%d,%d, %d,%d,%d,%d,%d,%d,%d,%d]},",
                        "{name:\"%s\", index:%d, DO:[%d,%d,%d,%d,%d,%d,%d,%d], DI:[%d,%d,%d,%d,%d,%d,%d,%d]},",
                             snap[i].name, snap[i].index, 
                             snap[i].u.io.DO[0],
                             snap[i].u.io.DO[1],
                             snap[i].u.io.DO[2],
                             snap[i].u.io.DO[3],

                             snap[i].u.io.DO[4],
                             snap[i].u.io.DO[5],
                             snap[i].u.io.DO[6],
                             snap[i].u.io.DO[7],

                             //snap[i].u.io.DO[8],
                             //snap[i].u.io.DO[9],
                             //snap[i].u.io.DO[10],
                             //snap[i].u.io.DO[11],

                             //snap[i].u.io.DO[12],
                             //snap[i].u.io.DO[13],
                             //snap[i].u.io.DO[14],
                             //snap[i].u.io.DO[15],

                             snap[i].u.io.DI[0],
                             snap[i].u.io.DI[1],
                             snap[i].u.io.DI[2],
                             snap[i].u.io.DI[3],

                             snap[i].u.io.DI[4],
                             snap[i].u.io.DI[5],
                             snap[i].u.io.DI[6],
                             snap[i].u.io.DI[7]

/*                             snap[i].u.io.DI[8],
                             snap[i].u.io.DI[9],
                             snap[i].u.io.DI[10],
                             snap[i].u.io.DI[11],

                             snap[i].u.io.DI[12],
                             snap[i].u.io.DI[13],
                             snap[i].u.io.DI[14],
                             snap[i].u.io.DI[15]  */                     
                            
                            );
                }
                else
                {
                    snprintf(tmp, sizeof(tmp),
                             "{name:\"%s\", index:%d, position:%d, velocity:%d, torque:%d, status:%d,  target_position:%d, running: %d},",
                             snap[i].name, 
                             snap[i].index, 
                             snap[i].u.servo.position,
                             snap[i].u.servo.velocity,
                             snap[i].u.servo.torque,
                             snap[i].u.servo.status,
                             snap[i].u.servo.target_position,
                             snap[i].u.servo.running
                            );
                }
                strcat(out, tmp);
            }
            strcat(out, "]}\n");
            send(g_cli, out, (int)strlen(out), 0);
        }
        sleep_ms(200);
    }
    return 0;
}

// ---- lifecycle ----
int tcpserver_start(void)
{
#ifdef _WIN32
    WSADATA wsa;
    if (WSAStartup(MAKEWORD(2, 2), &wsa) != 0)
        return -1;
#endif

    g_srv = socket(AF_INET, SOCK_STREAM, 0);
    if (g_srv < 0) return -2;

    struct sockaddr_in addr;
    memset(&addr, 0, sizeof(addr));
    addr.sin_family = AF_INET;
    addr.sin_addr.s_addr = htonl(INADDR_ANY);
    addr.sin_port = htons(Port);

    if (bind(g_srv, (struct sockaddr *)&addr, sizeof(addr)) < 0)
    {
        perror("bind(7070)");
        closesock_safe(&g_srv);
#ifdef _WIN32
        WSACleanup();
#endif
        return -3;
    }

    if (listen(g_srv, 1) < 0)
    {
        perror("listen");
        closesock_safe(&g_srv);
        return -4;
    }

    socket_set_nonblock(g_srv);
    g_cli = -1;
    g_running = 1;

#ifdef _WIN32
    th_accept = CreateThread(NULL, 0, th_accept_fn, NULL, 0, NULL);
    th_recv   = CreateThread(NULL, 0, th_recv_fn, NULL, 0, NULL);
    th_send   = CreateThread(NULL, 0, th_send_fn, NULL, 0, NULL);
#else
    pthread_create(&th_accept, NULL, th_accept_fn, NULL);
    pthread_create(&th_recv, NULL, th_recv_fn, NULL);
    pthread_create(&th_send, NULL, th_send_fn, NULL);
#endif
    return 0;
}

void tcpserver_stop(void)
{
    g_running = 0;
#ifdef _WIN32
    if (th_accept) { WaitForSingleObject(th_accept, INFINITE); CloseHandle(th_accept); th_accept=NULL; }
    if (th_recv)   { WaitForSingleObject(th_recv, INFINITE); CloseHandle(th_recv); th_recv=NULL; }
    if (th_send)   { WaitForSingleObject(th_send, INFINITE); CloseHandle(th_send); th_send=NULL; }
#else
    pthread_join(th_accept, NULL);
    pthread_join(th_recv, NULL);
    pthread_join(th_send, NULL);
#endif
    closesock_safe(&g_cli);
    closesock_safe(&g_srv);
#ifdef _WIN32
    WSACleanup();
#endif
}
